#include "StdAfx.h"
#include "Algorithm.h"

Algorithm::Algorithm(void)
{
}

Algorithm::~Algorithm(void)
{
}

float Algorithm::Bilinear(Point p, short *pOrgdata, const int nSizeX, const int nSizeY, const int nSizeZ)
{
	int           x, y, z;
	float         dx, dy, dz;
	int           u[8];
	float         I1, I2;
	float         J1, J2;
	float         W1, W2;
	float         IV;
	int           LayerSize;

    LayerSize = nSizeX * nSizeY;
	x = (int)p.x;
	y = (int)p.y;
	z = (int)p.z;
	
	if(x < nSizeX - 1 && y < nSizeY - 1 && z > 0 && z < nSizeZ - 1)
	{
		dx = p.x - (float)x;
		dy = p.y - (float)y;
		dz = p.z - (float)z;

		u[0] = z * nSizeX * nSizeY + y * nSizeX + x;
		u[1] = (z + 1) * nSizeX * nSizeY + y * nSizeX + x;
		u[2] = z * nSizeX * nSizeY + (y + 1) * nSizeX + x;
		u[3] = (z + 1) * nSizeX * nSizeY + (y + 1) * nSizeX + x;
		u[4] = z * nSizeX * nSizeY + y * nSizeX + x + 1;
		u[5] = (z + 1) * nSizeX * nSizeY + y * nSizeX + x + 1;
		u[6] = z * nSizeX * nSizeY + (y + 1) * nSizeX + x + 1;
		u[7] = (z + 1) * nSizeX * nSizeY + (y + 1) * nSizeX + x + 1;
		//first re-sample Z axis
		I1 = pOrgdata[u[0]] * (1 - dz) + pOrgdata[u[1]] * dz;
		I2 = pOrgdata[u[2]] * (1 - dz) + pOrgdata[u[3]] * dz;
		J1 = pOrgdata[u[4]] * (1 - dz) + pOrgdata[u[5]] * dz;
	    J2 = pOrgdata[u[6]] * (1 - dz) + pOrgdata[u[7]] * dz;
		//then re-sample Y axis;
		W1 = I1 * (1 - dy) + I2 * dy;
		W2 = J1 * (1 - dy) + J2 * dy;
		//at last, re-sample X axis;
	    IV = W1 * (1 - dx) + W2 * dx;
		
	}
	else
	{	
		int offer_3d = z * nSizeX * nSizeY + y * nSizeX + x;
	    IV = pOrgdata[offer_3d];
	}

	return IV;
}

float Algorithm::Cubic(Point p, short *pOrgdata, const int nSizeX, const int nSizeY, const int nSizeZ)
{

	int           x, y, z;
    register int  i, j, k;
	float         dx, dy, dz;
	float         u[4], v[4], w[4];
	float         r[4], q[4];
	float         vox = 0;
	int           LayerSize;

    LayerSize = nSizeX * nSizeY;
	x = (int)p.x;
	y = (int)p.y;
	z = (int)p.z;
	if(x < 0 || x >= nSizeX || y < 0 || y >= nSizeY || z < 0 || z >= nSizeZ)
		return (-3024.0);
	
	dx = p.x - (float)x;
	dy = p.y - (float)y;
	dz = p.z - (float)z;
	int nTempOffset = LayerSize * z + nSizeX * y + x;

	if(x > 0 && x < nSizeX - 1 && y > 0 && y < nSizeY - 1 && z > 0 && z < nSizeZ - 3)
	{

		u[0] = -0.5 * CUBE (dx) + SQR (dx) - 0.5 * dx;
		u[1] = 1.5 * CUBE (dx) - 2.5 * SQR (dx) + 1;
		u[2] = -1.5 * CUBE (dx) + 2 * SQR (dx) + 0.5 * dx;
		u[3] = 0.5 * CUBE (dx) - 0.5 * SQR (dx);

		v[0] = -0.5 * CUBE (dy) + SQR (dy) - 0.5 * dy;
		v[1] = 1.5 * CUBE (dy) - 2.5 * SQR (dy) + 1;
		v[2] = -1.5 * CUBE (dy) + 2 * SQR (dy) + 0.5 * dy;
		v[3] = 0.5 * CUBE (dy) - 0.5 * SQR (dy);

		w[0] = -0.5 * CUBE (dz) + SQR (dz) - 0.5 * dz;
		w[1] = 1.5 * CUBE (dz) - 2.5 * SQR (dz) + 1;
		w[2] = -1.5 * CUBE (dz) + 2 * SQR (dz) + 0.5 * dz;
		w[3] = 0.5 * CUBE (dz) - 0.5 * SQR (dz);

		for (k = 0; k < 4; k++)
		{
			q[k] = 0;
			for (j = 0; j < 4; j++)
			{
				r[j] = 0;
				for (i = 0; i < 4; i++)
				{
					r[j] += u[i] * pOrgdata[nTempOffset];
					nTempOffset++;
				}
				q[k] += v[j] * r[j];
				nTempOffset += nSizeX - 4;
			}
			vox += w[k] * q[k];
			nTempOffset += LayerSize - 4 * nSizeX;
		}
	}
	else
	{
		vox = pOrgdata[nTempOffset];
	}

	 return (vox < -3024.0 ? -3024.0 : vox);

}